#include "mapctrl.h"
#include <QDebug>
#include <QHostAddress>
#define MAPCTRL_PORT 1111
#define ADDR "192.168.2.125"
//#define test

MapCtrl::MapCtrl(QObject *parent) : QObject(parent)
{
    isMoving = false;
    skt = new QTcpSocket(this);
    connect(skt, SIGNAL(stateChanged(QAbstractSocket::SocketState)), this, SLOT(connectStateChanged(QAbstractSocket::SocketState)));
    connect(skt, SIGNAL(readyRead()), this, SLOT(readData()));

#ifdef test
    skt->connectToHost(QHostAddress(ADDR), 1111);
#else
    skt->connectToHost(QHostAddress::LocalHost, MAPCTRL_PORT);
#endif
}

//创建通信数据报文
QString MapCtrl::createPack(QString _type, QStringList params)
{
    QString param = QString(",%1,%2").arg(_type).arg(params.join(","));
    param = QString(",%1").arg(param.length()) + param;
    param = QString("#%1").arg(QString::number(JQChecksum::crc32(param.toLocal8Bit()), 16)) + param;
    return param;
    //    qDebug()<<param;
}

//从缓冲读取数据报文
void MapCtrl::readFromCache()
{
    int len = 0;
    QStringList tempList;
    int index[3];
    index[0] = dataCache.indexOf("#");
    if(index[0] == -1)
    {
        dataCache.clear();
        return;
    }
    if(index[0] !=0)
    {
        dataCache.remove(0, index[0]);
        readFromCache();
        return;
    }
    dataCache[0] = ',';

    tempList = dataCache.split(",", QString::SkipEmptyParts);

    //校验长度
    if(tempList.count()<3)
        return;
    len = tempList.at(1).toInt();
    if(len==0)
        return;
    index[1] = dataCache.indexOf(",", index[0]+1);
    index[2] = dataCache.indexOf(",", index[1]+1);

    if((len+index[2]) > dataCache.length())
        return;

    //校验值
    QString singlePack = dataCache.left(len+index[2]);
    dataCache.remove(0, len+index[2]);
    tempList = singlePack.split(",", QString::SkipEmptyParts);
    QByteArray qba = singlePack.right(singlePack.length() - index[1]).toLocal8Bit();
    qDebug()<<tempList.at(0)<<qba;
    QString localCheckSum = QString::number(JQChecksum::crc32(qba),16).toUpper();
    QString checkSum = tempList.at(0).toUpper();
    if(localCheckSum != checkSum)
    {
        qDebug()<<"[readFromCache]"<<"checkSum error.";
        qDebug()<<localCheckSum<<checkSum;
        return;
    }
    if(tempList.at(2) == "POS")
    {
        QPointF tarPos = QPointF(tempList.at(3).toDouble(),tempList.at(4).toDouble());
        int ang = (int)tempList.at(5).toDouble();
        emit carPos(tarPos, ang);
        if(tempList.at(6) == "MOVE")
            emit carState(true);
        else if(tempList.at(6) == "STOP")
            emit carState(false);
    }
    else if(tempList.at(2) == "LIDAR")
    {
        if(tempList.count() < 4)
            emit lidarUpdate(QVector<QPointF>());
        QVector<QPointF>lidarTars = getLidarInfo(tempList.mid(3));
        qDebug()<<lidarTars;
        if(!lidarTars.isEmpty())
        {
            emit lidarUpdate(lidarTars);
        }
    }
    else if(tempList.at(2) == "LIDAR_RAW")
    {
        if(tempList.count() < 4)
            emit lidarRawUpdate(QVector<QPointF>());
        QVector<QPointF>lidarTars = getLidarInfo(tempList.mid(3));
        qDebug()<<lidarTars;
        if(!lidarTars.isEmpty())
        {
            emit lidarRawUpdate(lidarTars);//x:distance  y:angle
        }
    }
    else if(tempList.at(2) == "CARINFO")
    {
        int params[3];
        memset(params, 0, sizeof(params));

        for(int i=0; i<(tempList.count()-3); i++)
        {
            params[i] = tempList[i+3].toInt();
        }
        emit carState(params[0], params[1], params[2]);
    }
    else
    {
        qDebug()<<"[singlePack]"<<singlePack;
    }
    tempList = dataCache.split("#");
    if(tempList.count()>0)
    {
        readFromCache();
    }
}

void MapCtrl::setCarState(bool ismove)
{
    if(ismove != isMoving)
    {
        isMoving = ismove;
    }
    emit carState(isMoving);
}

QVector<QPointF> MapCtrl::getLidarInfo(QStringList l)
{
    bool ok;
    QStringList pos;
    QVector<QPointF> ret;
    foreach(QString str, l)
    {
        pos = str.split(' ');
        if(pos.count() != 2)
            return QVector<QPointF>();

        QPointF p;
        p.setX(pos.at(0).toDouble(&ok));
        if(!ok)
            return QVector<QPointF>();

        p.setY(pos.at(1).toDouble(&ok));
        if(!ok)
            return QVector<QPointF>();
        ret.push_back(p);
    }
    return ret;
}

void MapCtrl::gotoPosReq(QPointF pos, int index)
{
//    QString params = QString("#GOTO#%1#%2").arg(QString::number(pos.x(), 'f')).arg(QString::number(pos.y(), 'f'));
//    QString strPack = QString("#%1%2").arg(params.length()).arg(params);
//    strPack = QString("#%1").arg(QString::number(JQChecksum::crc32(params.toLocal8Bit()), 16)) + strPack;
//    qDebug()<<"[simGoto]"<<strPack;
    QStringList params;
    params<<QString::number(pos.x(), 'f')<<QString::number(pos.y(), 'f')<<QString::number(index);
    QString param = createPack("GOTO", params);

    if(skt->write(param.toLocal8Bit()) > 0)
    {
//        history<<param;
    }
    qDebug()<<"[MapCtrl]"<<param;
}

void MapCtrl::carCtrlStart()
{
    QStringList params;
    params<<QString("START");
    QString param = createPack("CTRL", params);
    skt->write(param.toLocal8Bit());
    qDebug()<<"[MapCtrl]"<<param;
}

void MapCtrl::carCtrlStop()
{
    QStringList params;
    params<<QString("STOP");
    QString param = createPack("CTRL", params);
    skt->write(param.toLocal8Bit());
    qDebug()<<"[MapCtrl]"<<param;
}

void MapCtrl::connectStateChanged(QAbstractSocket::SocketState state)
{
    qDebug()<<"[connectStateChanged]"<<state;
    if(state == QAbstractSocket::UnconnectedState)
    {
        qDebug()<<"UnconnectedState";
        QTimer::singleShot(10000, this, SLOT(reconnect()));
    }
}

void MapCtrl::reconnect()
{
    skt->connectToHost(QHostAddress::LocalHost, MAPCTRL_PORT);
}

void MapCtrl::readData()
{
    QByteArray qba = skt->readAll();
    qDebug()<<"[MapCtrl::readData]:\n"<<qba;
    dataCache += QString(qba);
    readFromCache();
}
